/*
  stepper.c - stepper motor driver: executes motion plans using stepper motors

  Part of grblHAL

  Copyright (c) 2016-2024 Terje Io
  Copyright (c) 2011-2016 Sungeun K. Jeon for Gnea Research LLC
  Copyright (c) 2009-2011 Simen Svale Skogsrud

  grblHAL is free software: you can redistribute it and/or modify
  it under the terms of the GNU General Public License as published by
  the Free Software Foundation, either version 3 of the License, or
  (at your option) any later version.

  grblHAL is distributed in the hope that it will be useful,
  but WITHOUT ANY WARRANTY; without even the implied warranty of
  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
  GNU General Public License for more details.

  You should have received a copy of the GNU General Public License
  along with grblHAL. If not, see <http://www.gnu.org/licenses/>.
*/

#include <math.h>
#include <stdlib.h>
#include <string.h>

#include "hal.h"
#include "protocol.h"
#include "state_machine.h"

//#define MINIMIZE_PROBE_OVERSHOOT

//#include "debug.h"

//! \cond

// Some useful constants.
#define DT_SEGMENT (1.0f / (ACCELERATION_TICKS_PER_SECOND * 60.0f)) // min/segment
#define REQ_MM_INCREMENT_SCALAR 1.0f//1.25f

typedef enum {
    Ramp_Accel,
    Ramp_Cruise,
    Ramp_Decel,
    Ramp_DecelOverride
} ramp_type_t;

typedef union {
    uint8_t flags;
    struct {
        uint8_t velocity_profile   :1,
                hold_partial_block :1,
                parking            :1,
                decel_override     :1,// 当前块的距离不足以减速到目标速度,标记下一个块继续减速
                unassigned         :4;
    };
} prep_flags_t;

// Holds the planner block Bresenham algorithm execution data for the segments in the segment
// buffer. Normally, this buffer is partially in-use, but, for the worst case scenario, it will
// never exceed the number of accessible stepper buffer segments (SEGMENT_BUFFER_SIZE-1).
// NOTE: This data is copied from the prepped planner blocks so that the planner blocks may be
// discarded when entirely consumed and completed by the segment buffer. Also, AMASS alters this
// data for its own use.
static st_block_t st_block_buffer[SEGMENT_BUFFER_SIZE - 1];

// Primary stepper segment ring buffer. Contains small, short line segments for the stepper
// algorithm to execute, which are "checked-out" incrementally from the first block in the
// planner buffer. Once "checked-out", the steps in the segments buffer cannot be modified by
// the planner, where the remaining planner block steps still can.
static segment_t segment_buffer[SEGMENT_BUFFER_SIZE];

// Stepper ISR data struct. Contains the running data for the main stepper ISR.
static stepper_t st;

#ifdef ADAPTIVE_MULTI_AXIS_STEP_SMOOTHING
typedef struct {
    uint32_t level_1;
    uint32_t level_2;
    uint32_t level_3;
} amass_t;

static amass_t amass;
#endif

// Used for blocking new segments being added to the seqment buffer until deceleration starts
// after probe signal has been asserted.
static volatile bool probe_asserted = false;

// Stepper timer ticks per minute
static float cycles_per_min;

// Step segment ring buffer pointers
static volatile segment_t *segment_buffer_tail;
static segment_t *segment_buffer_head, *segment_next_head;

// Pointers for the step segment being prepped from the planner buffer. Accessed only by the
// main program. Pointers may be planning segments or planner blocks ahead of what being executed.
static plan_block_t *pl_block;     // Pointer to the planner block being prepped
static st_block_t *st_prep_block;  // Pointer to the stepper block data being prepped
static st_block_t st_hold_block;   // Copy of stepper block data for block put on hold during parking

// Segment preparation data struct. Contains all the necessary information to compute new segments
// based on the current executing planner block.
typedef struct {
    prep_flags_t recalculate;

    float dt_remainder;
    uint32_t steps_remaining;
    float steps_per_mm;
    float req_mm_increment;

    st_block_t *last_st_block;
    uint32_t last_steps_remaining;
    float last_steps_per_mm;
    float last_dt_remainder;

    ramp_type_t ramp_type;  // Current segment ramp state
    float mm_complete;      // End of velocity profile from end of current planner block in (mm).
                            // NOTE: This value must coincide with a step(no mantissa) when converted.
    float current_speed;    // Current speed at the end of the segment buffer (mm/min)
    float maximum_speed;    // Maximum speed of executing block. Not always nominal speed. (mm/min)
    float exit_speed;       // Exit speed of executing block (mm/min)
#ifdef KINEMATICS_API
    float rate_multiplier;  // Rate multiplier of executing block.
#endif
    float accelerate_until; // Acceleration ramp end measured from end of block (mm)
    float decelerate_after; // Deceleration ramp start measured from end of block (mm)
    float target_position;  //
    float target_feed;      //
    float inv_feedrate;     // Used by PWM laser mode to speed up segment calculations.
    float current_spindle_rpm;
} st_prep_t;

//! \endcond

static st_prep_t prep;

extern void gc_output_message (char *message);

/*    BLOCK VELOCITY PROFILE DEFINITION
          __________________________
         /|                        |\     _________________         ^
        / |                        | \   /|               |\        |
       /  |                        |  \ / |               | \       s
      /   |                        |   |  |               |  \      p
     /    |                        |   |  |               |   \     e
    +-----+------------------------+---+--+---------------+----+    e
    |               BLOCK 1            ^      BLOCK 2          |    d
                                       |
                  time ----->      EXAMPLE: Block 2 entry speed is at max junction velocity

  The planner block buffer is planned assuming constant acceleration velocity profiles and are
  continuously joined at block junctions as shown above. However, the planner only actively computes
  the block entry speeds for an optimal velocity plan, but does not compute the block internal
  velocity profiles. These velocity profiles are computed ad-hoc as they are executed by the
  stepper algorithm and consists of only 7 possible types of profiles: cruise-only, cruise-
  deceleration, acceleration-cruise, acceleration-only, deceleration-only, full-trapezoid, and
  triangle(no cruise).

                                        maximum_speed (< nominal_speed) ->  +
                    +--------+ <- maximum_speed (= nominal_speed)          /|\
                   /          \                                           / | \
 current_speed -> +            \                                         /  |  + <- exit_speed
                  |             + <- exit_speed                         /   |  |
                  +-------------+                     current_speed -> +----+--+
                   time -->  ^  ^                                           ^  ^
                             |  |                                           |  |
                decelerate_after(in mm)                             decelerate_after(in mm)
                    ^           ^                                           ^  ^
                    |           |                                           |  |
                accelerate_until(in mm)                             accelerate_until(in mm)

  The step segment buffer computes the executing block velocity profile and tracks the critical
  parameters for the stepper algorithm to accurately trace the profile. These critical parameters
  are shown and defined in the above illustration.
*/

//

// Callback from delay to deenergize steppers after movement, might been cancelled
void st_deenergize (void *data)
{
    if(sys.steppers_deenergize) {
        hal.stepper.enable(settings.steppers.energize, true);
        sys.steppers_deenergize = false;
    }
}

// Stepper state initialization. Cycle should only start if the st.cycle_start flag is
// enabled. Startup init and limits call this function but shouldn't start the cycle.
void st_wake_up (void)
{
    // Initialize stepper data to ensure first ISR call does not step and
    // cancel any pending steppers deenergize
    //st.exec_block = NULL;
    sys.steppers_deenergize = false;
    hal.stepper.wake_up();
}

// Stepper shutdown
ISR_CODE void ISR_FUNC(st_go_idle)(void)
{
    // Disable Stepper Driver Interrupt. Allow Stepper Port Reset Interrupt to finish, if active.

    sys_state_t state = state_get();

    hal.stepper.go_idle(false);

    // Set stepper driver idle state, disabled or enabled, depending on settings and circumstances.
    if(((settings.steppers.idle_lock_time != 255) || sys.rt_exec_alarm || state == STATE_SLEEP) && state != STATE_HOMING) {
        if(settings.steppers.idle_lock_time == 0 || state == STATE_SLEEP)
            hal.stepper.enable((axes_signals_t){0}, true);
        else {
            // Force stepper dwell to lock axes for a defined amount of time to ensure the axes come to a complete
            // stop and not drift from residual inertial forces at the end of the last movement.
            task_delete(st_deenergize, NULL); // Cancel any pending steppers deenergize task
            sys.steppers_deenergize = task_add_delayed(st_deenergize, NULL, settings.steppers.idle_lock_time);
        }
    } else
        hal.stepper.enable(settings.steppers.idle_lock_time == 255 ? (axes_signals_t){AXES_BITMASK} : settings.steppers.energize, true);
}


/* "The Stepper Driver Interrupt" - This timer interrupt is the workhorse of Grbl. Grbl employs
   the venerable Bresenham line algorithm to manage and exactly synchronize multi-axis moves.
   Unlike the popular DDA algorithm, the Bresenham algorithm is not susceptible to numerical
   round-off errors and only requires fast integer counters, meaning low computational overhead
   and maximizing the microcontrollers capabilities. However, the downside of the Bresenham algorithm
   is, for certain multi-axis motions, the non-dominant axes may suffer from un-smooth step
   pulse trains, or aliasing, which can lead to strange audible noises or shaking. This is
   particularly noticeable or may cause motion issues at low step frequencies (0-5kHz), but
   is usually not a physical problem at higher frequencies, although audible.
     To improve Bresenham multi-axis performance, Grbl uses what we call an Adaptive Multi-Axis
   Step Smoothing (AMASS) algorithm, which does what the name implies. At lower step frequencies,
   AMASS artificially increases the Bresenham resolution without effecting the algorithm's
   innate exactness. AMASS adapts its resolution levels automatically depending on the step
   frequency to be executed, meaning that for even lower step frequencies the step smoothing
   level increases. Algorithmically, AMASS is achieved by a simple bit-shifting of the Bresenham
   step count for each AMASS level. For example, for a Level 1 step smoothing, we bit shift
   the Bresenham step event count, effectively multiplying it by 2, while the axis step counts
   remain the same, and then double the stepper ISR frequency. In effect, we are allowing the
   non-dominant Bresenham axes step in the intermediate ISR tick, while the dominant axis is
   stepping every two ISR ticks, rather than every ISR tick in the traditional sense. At AMASS
   Level 2, we simply bit-shift again, so the non-dominant Bresenham axes can step within any
   of the four ISR ticks, the dominant axis steps every four ISR ticks, and quadruple the
   stepper ISR frequency. And so on. This, in effect, virtually eliminates multi-axis aliasing
   issues with the Bresenham algorithm and does not significantly alter Grbl's performance, but
   in fact, more efficiently utilizes unused CPU cycles overall throughout all configurations.
     AMASS retains the Bresenham algorithm exactness by requiring that it always executes a full
   Bresenham step, regardless of AMASS Level. Meaning that for an AMASS Level 2, all four
   intermediate steps must be completed such that baseline Bresenham (Level 0) count is always
   retained. Similarly, AMASS Level 3 means all eight intermediate steps must be executed.
   Although the AMASS Levels are in reality arbitrary, where the baseline Bresenham counts can
   be multiplied by any integer value, multiplication by powers of two are simply used to ease
   CPU overhead with bitshift integer operations.
     This interrupt is simple and dumb by design. All the computational heavy-lifting, as in
   determining accelerations, is performed elsewhere. This interrupt pops pre-computed segments,
   defined as constant velocity over n number of steps, from the step segment buffer and then
   executes them by pulsing the stepper pins appropriately via the Bresenham algorithm. This
   ISR is supported by The Stepper Port Reset Interrupt which it uses to reset the stepper port
   after each pulse. The bresenham line tracer algorithm controls all stepper outputs
   simultaneously with these two interrupts.

   NOTE: This interrupt must be as efficient as possible and complete before the next ISR tick,
   which for Grbl must be less than 33.3usec (@30kHz ISR rate). Oscilloscope measured time in
   ISR is 5usec typical and 25usec maximum, well below requirement.
   NOTE: This ISR expects at least one step to be executed per segment.
*/
// 基于Bresenham算法的多轴脉冲插补器
//! \cond
ISR_CODE void ISR_FUNC(stepper_driver_interrupt_handler)(void)
{
#if ENABLE_BACKLASH_COMPENSATION
    static bool backlash_motion;
#endif

    // Start a step pulse when there is a block to execute.
    // 这里处理的是块，只要有段就不会停止读取块
    if (st.exec_block) {                        // 检查是否有待执行的运动块（block）。
        hal.stepper.pulse_start(&st);           // 调用硬件抽象层（hal）中的步进电机控制函数，启动步进脉冲。这里就是和硬件直接挂钩的地方!!!!!!!!!!
        st.new_block = st.dir_change = false;   // 重置当前状态，标记为没有新的块（new_block）和方向变化（dir_change）。
        if (st.step_count == 0) {               // 如果步进计数为0，表示当前的段已经完成。
            st.exec_segment = NULL;             // 置空执行的段（exec_segment），意味着当前的执行段已经处理完毕，等待下一个段。
        }
    }

    // 如果没有步骤段（exec_segment），尝试从步骤缓冲区中弹出一个
    if (st.exec_segment == NULL) {
        // 缓冲区中是否有内容？如果有，加载并初始化下一个步骤段。
        if (segment_buffer_tail != segment_buffer_head) {
            // 初始化新的步骤段，并加载要执行的步数
            st.exec_segment = (segment_t *)segment_buffer_tail;
            /* hal.stepper.cycles_per_tick : 这个指针指向重装定时器初值的函数 
            也就是 TIM6->ARR = st.exec_segment->cycles_per_tick */
            hal.stepper.cycles_per_tick(st.exec_segment->cycles_per_tick);
            /* 以当前初值定时几次,为0时表示只要产生一个脉冲 */
            st.step_count = st.exec_segment->n_step;
            // 这里处理 st.exec_segment 段中的运动控制块

            // 在加减速算法中把一个块分割成了多个加减速段,这就存在连续几个段都属于同一个块,只是处于不同速度区间
            // 是同一个块的时候不需要更新运动方向，也不需要操作主轴相关的同步设置
            if (st.exec_block != st.exec_segment->exec_block) {// 当前块是否属于新的段
                // 如果方向发生了变化，更新方向
                if((st.dir_change = st.exec_block == NULL || st.dir_outbits.value != st.exec_segment->exec_block->direction_bits.value))
                    st.dir_outbits = st.exec_segment->exec_block->direction_bits;
                // 如果之前的执行块的 offset_id 和新块不同，则设置 wco（工作坐标系）
                /* TODO : 这里不太明确 */
                if(st.exec_block != NULL && st.exec_block->offset_id != st.exec_segment->exec_block->offset_id)
                    sys.report.wco = sys.report.force_wco = On; // 不生成 grbl.on_rt_reports_added 事件！
                // 更新执行块的状态
                st.exec_block = st.exec_segment->exec_block;
                /* 
                    st.step_event_count 从A点运动到B点时, 各个轴中移动距离最长的那根轴的距离 
                    被 AMASS处理过的
                    被 AMASS处理过的
                    被 AMASS处理过的
                */ 
                st.step_event_count = st.exec_block->step_event_count;
                st.new_block = true; // 表示已经进入了一个新块
    #if ENABLE_BACKLASH_COMPENSATION
                backlash_motion = st.exec_block->backlash_motion; // 如果启用了背隙补偿，处理背隙运动
    #endif
                // 如果当前执行块包含同步设置，应用这些设置
                /* TODO : 同步设置，不太清除会产生什么影响, 可以考虑移除 */
                if(st.exec_block->overrides.sync)
                    sys.override.control = st.exec_block->overrides;
                // 执行输出命令，使其与运动同步
                /*  这部分是用来控制风扇,冷却剂之类的IO */
                while(st.exec_block->output_commands) {
                    output_command_t *cmd = st.exec_block->output_commands;
                    cmd->is_executed = true;
                    // 根据命令类型执行相应的操作（数字输出或模拟输出）
                    if(cmd->is_digital)
                        hal.port.digital_out(cmd->port, cmd->value != 0.0f); // 设置数字端口输出
                    else
                        hal.port.analog_out(cmd->port, cmd->value); // 设置模拟端口输出
                    // 继续处理下一个命令
                    st.exec_block->output_commands = cmd->next;
                }
                // 如果有消息需要打印（通常是给前台进程处理的），将其加入队列
                // 在改造的时候,这部分需要处理，不然无法返回信息,但是不需要提取出来
                if(st.exec_block->message) {
                    if(!protocol_enqueue_foreground_task((foreground_task_ptr)gc_output_message, st.exec_block->message))
                        free(st.exec_block->message); // 如果无法入队，释放消息内存
                    st.exec_block->message = NULL;
                }
                // 初始化 Bresenham 线算法的坐标和步进计数器
                st.counter_x = st.counter_y = st.counter_z
                #ifdef A_AXIS
                    = st.counter_a
                #endif
                #ifdef B_AXIS
                    = st.counter_b
                #endif
                #ifdef C_AXIS
                    = st.counter_c
                #endif
                #ifdef U_AXIS
                    = st.counter_u
                #endif
                #ifdef V_AXIS
                    = st.counter_v
                #endif
                    = st.step_event_count >> 1; // A点到B点过程中运动距离最长的轴的移动距离 / 2

                // 如果没有启用自适应多轴步进平滑（AMASS），直接将步骤数组复制到当前块的步骤
                #ifndef ADAPTIVE_MULTI_AXIS_STEP_SMOOTHING
                    memcpy(st.steps, st.exec_block->steps, sizeof(st.steps));
                #endif
            }

    #ifdef ADAPTIVE_MULTI_AXIS_STEP_SMOOTHING
            // 自适应多轴步进平滑（Adaptive Multi-Axis Step Smoothing, AMASS）
            /* 
            先在假设 $100 = 20 $101 = 43.5, 机器处于 (0,0) 执行 G1 X10 Y2 F100 指令
            st.exec_block->steps[X_AXIS]中的值是(移动距离 * mm/step)<<amass_level,在这个例子中就是 (10 * 20) << 3
            在这里右移是得到左移前的原始值 
            */ 
            st.amass_level = st.exec_segment->amass_level;
            st.steps[X_AXIS] = st.exec_block->steps[X_AXIS] >> st.amass_level;
            st.steps[Y_AXIS] = st.exec_block->steps[Y_AXIS] >> st.amass_level;
            st.steps[Z_AXIS] = st.exec_block->steps[Z_AXIS] >> st.amass_level;
            #ifdef A_AXIS
                st.steps[A_AXIS] = st.exec_block->steps[A_AXIS] >> st.amass_level;
            #endif
            #ifdef B_AXIS
                st.steps[B_AXIS] = st.exec_block->steps[B_AXIS] >> st.amass_level;
            #endif
            #ifdef C_AXIS
                st.steps[C_AXIS] = st.exec_block->steps[C_AXIS] >> st.amass_level;
            #endif
            #ifdef U_AXIS
                st.steps[U_AXIS] = st.exec_block->steps[U_AXIS] >> st.amass_level;
            #endif
            #ifdef V_AXIS
                st.steps[V_AXIS] = st.exec_block->steps[V_AXIS] >> st.amass_level;
            #endif
    #endif
            // 根据步骤段是否包含 pwm 更新方法，更新激光或主轴的控制信号
            if(st.exec_segment->update_pwm)
                st.exec_segment->update_pwm(st.exec_block->spindle, st.exec_segment->spindle_pwm); // 更新主轴 pwm
            else if(st.exec_segment->update_rpm)
                st.exec_segment->update_rpm(st.exec_block->spindle, st.exec_segment->spindle_rpm); // 更新主轴 rpm
        } else {
            // 如果段缓冲区为空，系统进入空闲状态
            st_go_idle();
            // 确保在速率控制运动完成时，正确设置 pwm
            // 关闭激光
            if (st.exec_block->dynamic_rpm && st.exec_block->spindle->cap.laser)
                st.exec_block->spindle->update_pwm(st.exec_block->spindle, st.exec_block->spindle->pwm_off_value);
            // 清除执行块，标记系统为周期完成状态
            st.exec_block = NULL;
            system_set_exec_state_flag(EXEC_CYCLE_COMPLETE); // 标记主程序为周期完成
            return; // 如果没有任务，退出函数
        }
    }
    // 检查探针状态
    // 监控探针引脚状态，当探针被触发时记录当前系统位置。
    // 注意：该函数必须非常高效，避免在步进中断服务例程（ISR）中造成性能瓶颈。
    if (sys.probing_state == Probing_Active && hal.probe.get_state().triggered) {
        // 如果探针被触发，改变探针状态并记录当前系统位置。
        sys.probing_state = Probing_Off;
        memcpy(sys.probe_position, sys.position, sizeof(sys.position)); // 将当前位置记录到sys.probe_position
        bit_true(sys.rt_exec_state, EXEC_MOTION_CANCEL);                // 取消运动（表示探针操作完成）

    #ifdef MINIMIZE_PROBE_OVERSHOOT
        // 如果探针状态激活，并且缓冲区已满，则"清空"段缓冲区，以便提前开始减速。
        if((probe_asserted = segment_buffer_head->next == segment_buffer_tail)) {
            segment_buffer_head = segment_buffer_tail->next; // 将头指针移动到尾部的下一个段
            if(st.step_count < 3 || st.step_count < (st.exec_segment->n_step >> 3))
                segment_buffer_head = segment_buffer_head->next; // 进一步优化缓冲区
            segment_next_head = segment_next_head->next; // 更新下一段的头指针
        }
    #endif
    }

    // 初始化步进输出位
    register axes_signals_t step_outbits = (axes_signals_t){0};
    /*
        这段代码是 Bresenham 直线插补算法的核心实现，用于控制步进电机在多轴联动时生成平滑的直线轨迹。
        st.steps[X_AXIS]：X 轴在当前段的总步数（由运动指令和电机步距角决定）。
        st.step_event_count：主脉冲数，即所有轴中移动距离最长的轴的步数（作为基准）。
        st.counter_x：X 轴的累加计数器，用于动态判断何时输出一个脉冲。
        step_outbits.x：X 轴的脉冲输出标志，为On时触发硬件输出一个脉冲。
        sys.position[X_AXIS]：X 轴的当前位置（单位：脉冲数）。
        假设我们要控制 X 轴和 Y 轴从起点 (0,0) 移动到终点 (100,43)，即：
            X 轴总步数：st.steps[X_AXIS] = 100
            Y 轴总步数：st.steps[Y_AXIS] = 43
            主脉冲数：st.step_event_count = 100（X 轴步数最多）
        Bresenham 算法的核心是计算各轴的步进频率比例
            X 轴的步进频率：100%（每周期输出一个脉冲）
            Y 轴的步进频率：43/100 = 43%（每 100 个周期中输出 43 个脉冲）
        如何用整数运算实现 43% 的频率？累加比较法：
            初始化计数器：counter_y = 0
            每次周期：counter_y += 43
            当counter_y >= 100时，输出一个 Y 轴脉冲，并将counter_y -= 100
        这样，每 100 个周期中，Y 轴会输出 43 次脉冲，精确实现了 43% 的频率
    */
    st.counter_x += st.steps[X_AXIS];           // 更新X轴的计数器
    if (st.counter_x > st.step_event_count) {   // 如果计数器超过了步进事件计数
        step_outbits.x = On;                    // 标记X轴为有效步进
        st.counter_x -= st.step_event_count;    // 减去步进事件计数，准备下一步
    #if ENABLE_BACKLASH_COMPENSATION
        if(!backlash_motion) // 如果没有背隙补偿，则更新系统位置
    #endif
            // 更新逻辑位置
            sys.position[X_AXIS] = sys.position[X_AXIS] + (st.dir_outbits.x ? -1 : 1);
    }

    // Y轴类似于X轴，增加计数器并检查步进是否完成。
    st.counter_y += st.steps[Y_AXIS];
    if (st.counter_y > st.step_event_count) {
        step_outbits.y = On;
        st.counter_y -= st.step_event_count;
    #if ENABLE_BACKLASH_COMPENSATION
        if(!backlash_motion)
    #endif
            sys.position[Y_AXIS] = sys.position[Y_AXIS] + (st.dir_outbits.y ? -1 : 1);
    }

    // Z轴步骤，更新位置
    st.counter_z += st.steps[Z_AXIS];
    if (st.counter_z > st.step_event_count) {
        step_outbits.z = On;
        st.counter_z -= st.step_event_count;
    #if ENABLE_BACKLASH_COMPENSATION
        if(!backlash_motion)
    #endif
            sys.position[Z_AXIS] = sys.position[Z_AXIS] + (st.dir_outbits.z ? -1 : 1);
    }

    // A轴的处理
    #ifdef A_AXIS
        st.counter_a += st.steps[A_AXIS];
        if (st.counter_a > st.step_event_count) {
            step_outbits.a = On;
            st.counter_a -= st.step_event_count;
    #if ENABLE_BACKLASH_COMPENSATION
        if(!backlash_motion)
    #endif
            sys.position[A_AXIS] = sys.position[A_AXIS] + (st.dir_outbits.a ? -1 : 1);
        }
    #endif

    // B轴的处理
    #ifdef B_AXIS
        st.counter_b += st.steps[B_AXIS];
        if (st.counter_b > st.step_event_count) {
            step_outbits.b = On;
            st.counter_b -= st.step_event_count;
    #if ENABLE_BACKLASH_COMPENSATION
        if(!backlash_motion)
    #endif
            sys.position[B_AXIS] = sys.position[B_AXIS] + (st.dir_outbits.b ? -1 : 1);
        }
    #endif

    // C轴的处理
    #ifdef C_AXIS
        st.counter_c += st.steps[C_AXIS];
        if (st.counter_c > st.step_event_count) {
            step_outbits.c = On;
            st.counter_c -= st.step_event_count;
    #if ENABLE_BACKLASH_COMPENSATION
        if(!backlash_motion)
    #endif
            sys.position[C_AXIS] = sys.position[C_AXIS] + (st.dir_outbits.c ? -1 : 1);
        }
    #endif

    // U轴的处理
    #ifdef U_AXIS
        st.counter_u += st.steps[U_AXIS];
        if (st.counter_u > st.step_event_count) {
            step_outbits.u = On;
            st.counter_u -= st.step_event_count;
    #if ENABLE_BACKLASH_COMPENSATION
        if(!backlash_motion)
    #endif
            sys.position[U_AXIS] = sys.position[U_AXIS] + (st.dir_outbits.u ? -1 : 1);
        }
    #endif

    // V轴的处理
    #ifdef V_AXIS
        st.counter_v += st.steps[V_AXIS];
        if (st.counter_v > st.step_event_count) {
            step_outbits.v = On;
            st.counter_v -= st.step_event_count;
    #if ENABLE_BACKLASH_COMPENSATION
        if(!backlash_motion)
    #endif
            sys.position[V_AXIS] = sys.position[V_AXIS] + (st.dir_outbits.v ? -1 : 1);
        }
    #endif

    // 更新步进输出位（最终的步进信号）
    st.step_outbits.value = step_outbits.value;

    // 在归位周期中，锁定并防止某些轴的移动。
    if (state_get() == STATE_HOMING)
        st.step_outbits.value &= sys.homing_axis_lock.mask; // 使用归位轴锁定屏蔽，限制轴的运动

    // 如果步进计数为0，或步进计数递减到0，表示当前步骤段已完成。
    // 此时，更新缓冲区的尾指针，指向下一个步骤段。
    // NOTE : 注意 --st.step_count 这个操作
    if (st.step_count == 0 || --st.step_count == 0) {
        // 完成当前步骤段，移动段缓冲区的尾指针
        segment_buffer_tail = segment_buffer_tail->next;
    }
}

//! \endcond

// Reset and clear stepper subsystem variables
void st_reset (void)
{
    if(hal.probe.configure)
        hal.probe.configure(false, false);

    // Initialize stepper driver idle state, clear step and direction port pins.
    st_go_idle();
   // hal.stepper.go_idle(true);

    // NOTE: buffer indices starts from 1 for simpler driver coding!

    // Set up stepper block ringbuffer as circular linked list and add id
    uint_fast8_t idx, idx_max = (sizeof(st_block_buffer) / sizeof(st_block_t)) - 1;
    for(idx = 0 ; idx <= idx_max ; idx++) {
        st_block_buffer[idx].next = &st_block_buffer[idx == idx_max ? 0 : idx + 1];
        st_block_buffer[idx].id = idx + 1;
    }

    // Set up segments ringbuffer as circular linked list, add id and clear AMASS level
    idx_max = (sizeof(segment_buffer) / sizeof(segment_t)) - 1;
    for(idx = 0 ; idx <= idx_max ; idx++) {
        segment_buffer[idx].next = &segment_buffer[idx == idx_max ? 0 : idx + 1];
        segment_buffer[idx].id = idx + 1;
        segment_buffer[idx].amass_level = 0;
    }

    st_prep_block = &st_block_buffer[0];

    // Initialize stepper algorithm variables.
    pl_block = NULL;  // Planner block pointer used by segment buffer
    segment_buffer_tail = segment_buffer_head = &segment_buffer[0]; // empty = tail
    segment_next_head = segment_buffer_head->next;

    memset(&prep, 0, sizeof(st_prep_t));
    memset(&st, 0, sizeof(stepper_t));

#ifdef ADAPTIVE_MULTI_AXIS_STEP_SMOOTHING
    // TODO: move to driver?
    // AMASS_LEVEL0: Normal operation. No AMASS. No upper cutoff frequency. Starts at LEVEL1 cutoff frequency.
    // Defined as step timer frequency / Cutoff frequency in Hz
    amass.level_1 = hal.f_step_timer / 8000;
    amass.level_2 = hal.f_step_timer / 4000;
    amass.level_3 = hal.f_step_timer / 2000;
#endif

    cycles_per_min = (float)hal.f_step_timer * 60.0f;
}

// Called by spindle_set_state() to inform about RPM changes.
// Used by st_prep_buffer() to determine if spindle needs update when dynamic RPM is called for.
void st_rpm_changed (float rpm)
{
    prep.current_spindle_rpm = rpm;
}

// Called by planner_recalculate() when the executing block is updated by the new plan.
void st_update_plan_block_parameters (void)
{
    if (pl_block != NULL) { // Ignore if at start of a new block.
        prep.recalculate.velocity_profile = On;
        pl_block->entry_speed_sqr = prep.current_speed * prep.current_speed; // Update entry speed.
        pl_block = NULL; // Flag st_prep_segment() to load and check active velocity profile.
    }
}

// Changes the run state of the step segment buffer to execute the special parking motion.
void st_parking_setup_buffer (void)
{
    // Store step execution data of partially completed block, if necessary.
    if (prep.recalculate.hold_partial_block && !prep.recalculate.parking) {
        prep.last_st_block = st_prep_block;
        memcpy(&st_hold_block, st_prep_block, sizeof(st_block_t));
        prep.last_steps_remaining = prep.steps_remaining;
        prep.last_dt_remainder = prep.dt_remainder;
        prep.last_steps_per_mm = prep.steps_per_mm;
    }
    // Set flags to execute a parking motion
    prep.recalculate.parking = On;
    prep.recalculate.velocity_profile = Off;
    pl_block = NULL; // Always reset parking motion to reload new block.
}


// Restores the step segment buffer to the normal run state after a parking motion.
void st_parking_restore_buffer (void)
{
    // Restore step execution data and flags of partially completed block, if necessary.
    if (prep.recalculate.hold_partial_block) {
        memcpy(prep.last_st_block, &st_hold_block, sizeof(st_block_t));
        st_prep_block = prep.last_st_block;
        prep.steps_remaining = prep.last_steps_remaining;
        prep.dt_remainder = prep.last_dt_remainder;
        prep.steps_per_mm = prep.last_steps_per_mm;
        prep.recalculate.flags = 0;
        prep.recalculate.hold_partial_block = prep.recalculate.velocity_profile = On;
        prep.req_mm_increment = REQ_MM_INCREMENT_SCALAR / prep.steps_per_mm; // Recompute this value.
    } else
        prep.recalculate.flags = 0;

    pl_block = NULL; // Set to reload next block.
}

/* Prepares step segment buffer. Continuously called from main program.

   The segment buffer is an intermediary buffer interface between the execution of steps
   by the stepper algorithm and the velocity profiles generated by the planner. The stepper
   algorithm only executes steps within the segment buffer and is filled by the main program
   when steps are "checked-out" from the first block in the planner buffer. This keeps the
   step execution and planning optimization processes atomic and protected from each other.
   The number of steps "checked-out" from the planner buffer and the number of segments in
   the segment buffer is sized and computed such that no operation in the main program takes
   longer than the time it takes the stepper algorithm to empty it before refilling it.
   Currently, the segment buffer conservatively holds roughly up to 40-50 msec of steps.
   NOTE: Computation units are in steps, millimeters, and minutes.
   
    st_prep_buffer 函数在 GRBL 中的作用是准备 步进电机步进段缓冲区（step segment buffer）。
    这个缓冲区作为步进电机控制算法和规划器（planner）之间的中介接口。
    该函数确保在步进电机执行每个步进命令时，能够保证规划器和执行器的独立性和同步性。
    加减速规划就在这里 在 速度前瞻结束后会调用，然后 protocol_execute_realtime ---> protocol_exec_rt_system--->st_prep_buffer
    protocol_execute_realtime - 这个函数的调用到处都是
*/
void st_prep_buffer (void)
{
    // Block step prep buffer, while in a suspend state and there is no suspend motion to execute.
    if (sys.step_control.end_motion)
        return;
    //在 void st_reset (void) 中 segment_buffer_tail != segment_next_head这个条件就成立了
    // 也就是默认情况下这个循环能进来
    while (segment_buffer_tail != segment_next_head) { // Check if we need to fill the buffer.
        // Determine if we need to load a new planner block or if the block needs to be recomputed.
        if (pl_block == NULL) {

            // Query planner for a queued block
            // !!!!!!!!!!!!!!! 这里跟static void planner_recalculate (void)衔接起来了
            pl_block = sys.step_control.execute_sys_motion ? plan_get_system_motion_block() : plan_get_current_block();

            if (pl_block == NULL)
                return; // No planner blocks. Exit.

            // Check if we need to only recompute the velocity profile or load a new block.
            // prep 是 Preprocessing 预处理的意思
            if (prep.recalculate.velocity_profile) { // 只需要重新计算速度剖面?
                if(settings.parking.flags.enabled) { // $41 急停是否启用
                    if (prep.recalculate.parking)
                        prep.recalculate.velocity_profile = Off;
                    else
                        prep.recalculate.flags = 0;
                } else
                    prep.recalculate.flags = 0;
            } else {// 在这里速度前瞻后的数据就在这跟加减速规划搭上关系了

                // Prepare and copy Bresenham algorithm segment data from the new planner block, so that
                // when the segment buffer completes the planner block, it may be discarded when the
                // segment buffer finishes the prepped block, but the stepper ISR is still executing it.
                // 准备并复制Bresenham算法段数据，从新的规划块中获取数据。
                // 这样，当段缓冲区完成计划块时，可以丢弃它，尽管步进中断服务程序（ISR）仍在执行。
                // 初始化的时候这样设置了 st_prep_block = &st_block_buffer[0];
                st_prep_block = st_prep_block->next;  // 移动到下一个准备块

                uint_fast8_t idx = N_AXIS;  // 轴数初始化
                #ifndef ADAPTIVE_MULTI_AXIS_STEP_SMOOTHING
                    do {
                        idx--;  // 对所有轴数据进行循环
                        st_prep_block->steps[idx] = (pl_block->steps[idx] << 1);  // 将步进数加倍(以脉冲数为单位)
                    } while(idx);  // 循环至所有轴
                    st_prep_block->step_event_count = (pl_block->step_event_count << 1);  // 事件数也加倍(脉冲数)
                #else
                    // 启用AMASS时，按最大AMASS等级将所有Bresenham数据位移，避免在算法中除法运算
                    // 自适应多轴步进平滑 (AMASS): 将步进数据根据最大AMASS等级进行移位操作，避免在算法中进行除法运算，
                    //以防因整数四舍五入丢失步进
                    do {
                        idx--;
                        st_prep_block->steps[idx] = pl_block->steps[idx] << MAX_AMASS_LEVEL;  // 每个轴要发出的脉冲数都放大
                    } while(idx);  // 循环至所有轴
                    // 在 插补函数中有  st.step_event_count = st.exec_block->step_event_count;
                    st_prep_block->step_event_count = pl_block->step_event_count << MAX_AMASS_LEVEL;  // 本段运动轴中位移最大的那个轴的脉冲数等比例放大
                #endif

                // 复制其他相关参数到准备块
                // 这种值传导到segment的exec_block中
                st_prep_block->direction_bits = pl_block->direction_bits;       // 方向信息
                st_prep_block->programmed_rate = pl_block->programmed_rate;     // Gcode中指定的速度(mm/min)
                st_prep_block->millimeters = pl_block->millimeters;             // 矢量合成运动的模长距离(mm)
                // pl_block->step_event_count：位移最大的轴的总脉冲数（没有经过 AMASS 处理,以脉冲数量为单位）。
                // pl_block->millimeters：合成矢量方向的总运动距离（单位：毫米）。
                // st_prep_block->steps_per_mm 这个得到的是本次矢量位移每1mm占多少个脉冲
                st_prep_block->steps_per_mm = (float)pl_block->step_event_count / pl_block->millimeters;//(step/mm)
                st_prep_block->spindle = pl_block->spindle.hal;                 // 主轴信息
                st_prep_block->output_commands = pl_block->output_commands;     // 输出命令
                st_prep_block->overrides = pl_block->overrides;                 // 覆盖设置
                st_prep_block->offset_id = pl_block->offset_id;                 // 偏移ID
                st_prep_block->backlash_motion = pl_block->condition.backlash_motion;  // 回程误差运动状态
                st_prep_block->message = pl_block->message;  // 消息信息
                pl_block->message = NULL;  // 清空消息

                // 初始化段缓冲区数据，用于生成段
                prep.steps_per_mm = st_prep_block->steps_per_mm;  // 合成矢量运动方向上每毫米要几个脉冲
                prep.steps_remaining = pl_block->step_event_count; /* 这里标记为剩余步数，一开始没运动，所以初始化成最大步数，后面应该会减到零。*/
                /* prep.req_mm_increment: 每个脉冲几mm
                    REQ_MM_INCREMENT_SCALAR = 1时表示，合成矢量方向上每个脉冲多少mm
                    REQ_MM_INCREMENT_SCALAR = 1.25 的目的不太清楚,但是这里会导致精度略低
                        可能是GRBL针对嵌入式系统资源受限场景的优化策略
                */
                prep.req_mm_increment = REQ_MM_INCREMENT_SCALAR / prep.steps_per_mm; 
                prep.dt_remainder = prep.target_position = 0.0f;  // 重置目标位置和剩余时间
                #ifdef KINEMATICS_API // 这个宏跳过
                    prep.rate_multiplier = pl_block->rate_multiplier;  // 速率乘数
                #endif

                // 如果系统正在执行保持或需要覆盖减速，设置当前速度为退出速度
                if (sys.step_control.execute_hold || prep.recalculate.decel_override) {
                    prep.current_speed = prep.exit_speed;                           // 上一刻的退出速度设置为当前速度
                    pl_block->entry_speed_sqr = prep.exit_speed * prep.exit_speed;  // 入口速度 = 当前速度 = 上一刻的退出速度
                    prep.recalculate.decel_override = Off;                          // 关闭减速覆盖
                } else
                    // pl_block->entry_speed_sqr 这个入口速度在 static void planner_recalculate (void)中确定的,一开始是 0
                    prep.current_speed = sqrtf(pl_block->entry_speed_sqr);  // 否则，使用入口速度的平方根作为当前速度
                
                if ((st_prep_block->dynamic_rpm = pl_block->condition.is_rpm_rate_adjusted)) {
                    // RPM 速率调整模式
                    // RPM_实际 = RPM_设定 × (当前速度 / 编程速度)
                    // PPI (Pulse Per Inch) 模式：激光功率基于距离而非速度调整
                    // 预计算编程速率的倒数，避免实时计算除法
                    prep.inv_feedrate = pl_block->condition.is_laser_ppi_mode ? 1.0f : 1.0f / pl_block->programmed_rate;
                } else
                    // CSS (恒定表面速度) 模式;在激光设备中，表面恒速度模式没有用处
                    st_prep_block->dynamic_rpm = !!pl_block->spindle.css;  // 根据主轴CSS状态设置动态RPM
                    
            }

            /* ---------------------------------------------------------------------------------
             Compute the velocity profile of a new planner block based on its entry and exit
             speeds, or recompute the profile of a partially-completed planner block if the
             planner has updated it. For a commanded forced-deceleration, such as from a feed
             hold, override the planner velocities and decelerate to the target exit speed.
            */
            // 设置初始的运动完成度为0.0mm，表示速度曲线默认情况下从块的起始位置开始，尚未完成。
            // 遇到暂停的时候，在减速到0的过程如果当前块没有被消完,会把没有消耗的部分放在这个变量里面
            // 或许可以叫累计剩余距离
            prep.mm_complete = 0.0f; 
            // 这里是算法优化
            // 1 / (2a)，正常算的话有一个乘法和一个除法
            // 但是这个式子等价于 1 / 2 / a = 0.5 / a
            // pl_block->acceleration - 取联动轴中加速度最小的那个轴(mm/min^2)
            float inv_2_accel = 0.5f / pl_block->acceleration;


            //  prep.ramp_type 这个很重要,用来标记是加速度端，匀速运动段，还是减速度段
            if (sys.step_control.execute_hold) { // 如果正在进行进给停止操作（Feed Hold）
                // 强制减速，目标是将速度减小到零。
                prep.ramp_type = Ramp_Decel; // 设置速度剖面为减速模式。
                
                // 计算从当前块结束位置开始的减速距离。
                // pl_block->millimeters ：合成矢量方向的总运动距离（单位：毫米）。
                // 这里是比较当前剩余距离和需要的减速距离的差值判断当前距离够不够减速到减速距离
                // 减速度公式 : V^2 = V0^2 - 2as; 要减速到0 所以 V^2 = 0
                // s = V0^2 / 2a = ( 1 / (2a) ) * V0^2
                // 1/(2a) = inv_2_accel
                // V0^2 = pl_block->entry_speed_sqr
                float decel_dist = pl_block->millimeters - inv_2_accel * pl_block->entry_speed_sqr;//(mm/min)^2
                if(decel_dist < -0.0001f) {// 减速距离不够
                    // 这个公式是基于匀变速直线运动公式：v^2 = v0^2 + 2*a*s
                    // 得到当前段能减速到的什么速度
                    prep.exit_speed = sqrtf(pl_block->entry_speed_sqr - 2.0f * pl_block->acceleration * pl_block->millimeters);
                } else {
                    // decel_dist < 0.0001f 成立 距离够减速到0
                    // 标记减速完成后剩下的距离
                    prep.mm_complete = decel_dist < 0.0001f ? 0.0f : decel_dist;
                    prep.exit_speed = 0.0f; // 退出速度为零。
                }
            } else { // 正常操作模式
                // 计算或重新计算速度曲线参数。
                prep.ramp_type = Ramp_Accel;                    // 初始化为加速剖面。
                prep.accelerate_until = pl_block->millimeters;  // 这应该是初始化，该段内从入口速度到达出口速度后还剩多少距离没有用完,后续的计算会覆盖这个值
                float exit_speed_sqr;                           // 退出速度的平方。
                if (sys.step_control.execute_sys_motion)        // 是回零运动
                    prep.exit_speed = exit_speed_sqr = 0.0f;    // 退出速度强制设置为0
                else {
                    // 读取下一个块的开始速度,就是当前块的退出速度
                    exit_speed_sqr = plan_get_exec_block_exit_speed_sqr();
                    prep.exit_speed = sqrtf(exit_speed_sqr);
                }

                // 约束后能允许的最大速度
                float nominal_speed = plan_compute_profile_nominal_speed(pl_block);
                float nominal_speed_sqr = nominal_speed * nominal_speed;
                /*
                    在一个段中可能有加速过程和减速过程
                    加速度是 V^2 = V0^2 + 2as; 减速度是 V^2 = V0^2 - 2as;
                    判断是减速还是减速
                        减速 : (V0^2 - V^2)
                        加速 : (V^2 - V0^2)
                    总的位移为 d (pl_block->millimeters)
                    入口速度 V_entry (pl_block->entry_speed_sqr)
                    出口速度 v_exit (exit_speed_sqr)
                    加速度 a ( inv_2_accel : 1 / 2a )
                    交点距离 s (float intersect_distance)
                    加速段:
                        Vmax^2 = V_entry^2 + 2as
                    减速段:0                        v_exit^2 = Vmax^2 - 2a(d - s)
                    消掉 Vmax^2 化简得到
                    s = 1/2 * ( d + ( V_entry^2 - v_exit^2 ) / 2a )
                    结果 >0 → 存在加速段,
                    结果 >0 > 总行程 : 纯减速
                    结果 >0 && 结合其他条件 → 加速后减速的情况
                    结果 ≤0 → 无法形成加速段,入口速度为0 出口速度500mm/s ( d < 12.5 ) 
                                意味着本段行程不足以达到目标速度,
                    例如 ：
                        入口速度 500mm/s 出口速度0 a=10000mm/s^2 d=25
                        那应该先加速到612.37mm/s 然后减速,到出口时正好为0
                        那么 6.25mm 加速 18.75mm 减速
                 */
                float intersect_distance = 0.5f * (pl_block->millimeters + inv_2_accel * (pl_block->entry_speed_sqr - exit_speed_sqr));

                prep.target_feed = nominal_speed; // 设置目标进给速度。
                /*
                    速度前瞻工作的时候这个条件不会成立
                    在寻边和回零或者只有一条指令的时候因为退出速度强制为0,所以入口速度为0,
                    出口速度为0,假设 d 取 1500mm
                    那 V = sqrt(2*1500*500 = 1000000) = 1224.7 会超过机器允许的最大速度1000mm/s
                 */
                if (pl_block->entry_speed_sqr > nominal_speed_sqr) {// 入口速度大于给定速度,要减速
                    // 这里存在两种情况, 纯减速和 减速后匀速,再减速
                    /*
                        计算减速阶段结束位置
                        inv_2_accel * (pl_block->entry_speed_sqr - nominal_speed_sqr) ： 从入口速度加速到出口速度要的距离
                        pl_block->millimeters : 本段可用的距离
                        prep.accelerate_until : 从入口速度减速到目标速度后本段还有多少距离没用完; 这个包含了匀速段(如果有)和减速段
                    */
                    prep.accelerate_until = pl_block->millimeters - inv_2_accel * (pl_block->entry_speed_sqr - nominal_speed_sqr);

                    if (prep.accelerate_until <= 0.0f) { // 只有减速
                        prep.ramp_type = Ramp_Decel; // 设置为减速模式
                        // 得到当前段能减速到的什么速度
                        prep.exit_speed = sqrtf(pl_block->entry_speed_sqr - 2.0f * pl_block->acceleration * pl_block->millimeters);
                        prep.recalculate.decel_override = On; // 标记下一个块继续减速
                    } else {
                        // 得到减速到目标速度需要的距离
                        // nominal_speed_sqr - 允许的最大速度
                        // exit_speed_sqr - 退出速度
                        prep.decelerate_after = inv_2_accel * (nominal_speed_sqr - exit_speed_sqr);
                        prep.maximum_speed = nominal_speed;   // 约束后能允许的最大速度
                        prep.ramp_type = Ramp_DecelOverride;  // 设置为减速覆盖模式
                    }
                } else if (intersect_distance > 0.0f) { // 存在加速后减速的情况,或者匀速减速
                    if (intersect_distance < pl_block->millimeters) {// 这个加减速交界点在本段行程内
                        // 得到减速到目标速度需要的距离
                        prep.decelerate_after = inv_2_accel * (nominal_speed_sqr - exit_speed_sqr);// 当前速度到最大速度要的距离(exit_speed_sqr 上一刻的退出速度就是当前的入口速度)
                        if (prep.decelerate_after < intersect_distance) {/*
                            减速到出口速度所需的距离 小于 加减速切换点位置
                                意味着运动过程包含三个阶段
                                加速段：从入口速度加速到最大允许速度
                                匀速段：以最大允许速度巡航
                                减速段：从最大允许速度减速到出口速度                           
                            */ 
                            prep.maximum_speed = nominal_speed;         // 让运动保持允许的最大速度
                            if (pl_block->entry_speed_sqr == nominal_speed_sqr) {// 入口速度 等于 最大速度
                                // 不需要加速，后续匀速运动就行
                                prep.ramp_type = Ramp_Cruise; // 匀速运动
                            } else {
                                // 计算加速度所需要的距离,并从剩余距离中减掉这部分距离
                                // 在最开始的时候设置成了加速剖面
                                prep.accelerate_until -= inv_2_accel * (nominal_speed_sqr - pl_block->entry_speed_sqr);
                            }
                        } else { /* 减速到出口速度所需的距离 大于等于 加减速切换点位置
                                    意味着
                                        没有足够空间加速到最大允许速度
                                        必须在达到最大允许速度之前开始减速
                                        运动简化为两阶段：加速 → 立即减速（无匀速段）                        
                         */ 
                            //暗含 prep.ramp_type = Ramp_Accel;
                            /* 
                                将加速结束位置和减速开始位置设为同一个点,取消匀速段，直接在该点从加速切换到减速
                                prep.accelerate_until - 运动块起始到结束的距离
                                prep.decelerate_after - 运动起点到减速点的距离
                                intersect_distance    - 是加速与减速阶段的切换点
                            */
                            prep.accelerate_until = prep.decelerate_after = intersect_distance;
                            /* 
                                v^2 = V0^2 - 2as
                                v0 = sqrt( v^2 + 2as )
                                这里是为了重新约束最大速度
                             */ 
                            prep.maximum_speed = sqrtf(2.0f * pl_block->acceleration * intersect_distance + exit_speed_sqr);
                        }
                    } else {// 加减速交界点在本段外,纯减速
                        prep.ramp_type = Ramp_Decel;
                    }
                } else {
                    //暗含 prep.ramp_type = Ramp_Accel;
                    // 本段行程内不足以从入口速度加速到出口速度
                    // 加速完成后不会剩下任何距离,能达到的最大速度就是出口速度
                    prep.accelerate_until = 0.0f;
                    prep.maximum_speed = prep.exit_speed;
                }
            }

            // 如果系统没有处于归位状态，并且是激光主轴,标记一下后面让后面处理
            if(state_get() != STATE_HOMING)
                sys.step_control.update_spindle_rpm |= pl_block->spindle.hal->cap.laser; 

            // 禁止探针的触发
            probe_asserted = false;
        }

        // Block adding new segments after probe is asserted until deceleration is started.
        if(probe_asserted)
            return;

        // Initialize new segment
        segment_t *prep_segment = segment_buffer_head;

        // Set new segment to point to the current segment data block.
        // st_prep_block = &st_block_buffer
        prep_segment->exec_block = st_prep_block;
        prep_segment->update_rpm = NULL;
        prep_segment->update_pwm = NULL;

        /*------------------------------------------------------------------------------------
            Compute the average velocity of this new segment by determining the total distance
          traveled over the segment time DT_SEGMENT. The following code first attempts to create
          a full segment based on the current ramp conditions. If the segment time is incomplete
          when terminating at a ramp state change, the code will continue to loop through the
          progressing ramp states to fill the remaining segment execution time. However, if
          an incomplete segment terminates at the end of the velocity profile, the segment is
          considered completed despite having a truncated execution time less than DT_SEGMENT.
            The velocity profile is always assumed to progress through the ramp sequence:
          acceleration ramp, cruising state, and deceleration ramp. Each ramp's travel distance
          may range from zero to the length of the block. Velocity profiles can end either at
          the end of planner block (typical) or mid-block at the end of a forced deceleration,
          such as from a feed hold.
        */
       /* DT_SEGMENT 人为给定 delta t 这个值越小越好;决定了时间颗粒度 */
        float dt_max = DT_SEGMENT; // 最大的 delta t (min/segment) 这个变量也决定了计算间隔,必须在这个间隔内完成计算
        float dt = 0.0f;
        float time_var = dt_max;/* 加速度不同，走同样的距离需要的时间也不同, 这个是可变的 delta t */
        float mm_var;
        float speed_var;
        /* 
            pl_block->millimeters 一开始是矢量合成运动的模长距离
            pl_block->millimeters = mm_remaining; 结束的时候会更新剩余距离,并不总是等于模长
            所以这里 mm_remaining = pl_block->millimeters 只要距离没有彻底执行完，那每次都缩短一点点,具体跟加速度和速度有关
        */
        float mm_remaining = pl_block->millimeters;


        // prep.req_mm_increment - 矢量合成方向上 1个脉冲是多少mm
        // 这样做是因为下面是do{} while() 至少都会执行一次，所以要减去一个脉冲(单位被统一成mm)
        float minimum_mm = mm_remaining - prep.req_mm_increment;

        if (minimum_mm < 0.0f)
            minimum_mm = 0.0f; // 如果最小所需距离为负数，则设置为零
        do {
            // 根据不同的加速、巡航和减速状态来切换处理逻辑
            switch (prep.ramp_type) {
                case Ramp_DecelOverride:// 减速后切到匀速
                    /*原式 : a = delta_v / delta_t
                    delta_v = a * delta_t */
                    speed_var = pl_block->acceleration * time_var;
                    /* prep.current_speed - prep.maximum_speed : 本次的 delta t
                    speed_var : 理论上单位时间内的 delta t
                    两个delta t 比较，要是实际的速度变化量比理论上的单位时间内的速度变化小,则认为速度没有变化,减速完成 */
                    if ( ( prep.current_speed - prep.maximum_speed ) <= speed_var ) {// 减速完成 ?
                        mm_remaining = prep.accelerate_until;// 更新没有使用完的距离,这里依赖前面的计算
                        /* pl_block->millimeters    - 总长度
                        mm_remaining                - 剩余的长度
                        prep.current_speed          - 当前速度
                        prep.maximum_speed          - 最大速度
                        公式为 : S = (V0 + V)/2 * t 这个只适用于匀速直线运动
                        以[ 平均速度 (prep.current_speed + prep.maximum_speed)/2 ] 经过 [ 距离 pl_block->millimeters - mm_remaining ] 所需的时间 
                        */ 
                       //在时间切片内,一小段是减速段，另一小段是匀速段, time_var 在这里记录减速段消耗的时间
                        time_var = 2.0f * ( pl_block->millimeters - mm_remaining ) / ( prep.current_speed + prep.maximum_speed );
                        prep.ramp_type = Ramp_Cruise;               // 切换到巡航阶段
                        prep.current_speed = prep.maximum_speed;    // 设置当前速度为最大速度
                    } else {
                        /* 位移公式为 delta_s = v0 * t + 1/2 * a * t^2
                        减速的时候 delta_s = v0 * t - 1/2 * a * t^2
                        提出一个 t 变成 delta_s = t( v0 - 1/2 * a * t )
                            与 v = at 联立
                        得到 delta_s = t( v0 - 0.5 * v )
                        与 time_var * (prep.current_speed - 0.5f * speed_var) 一致 ( 平均速度 ) */
                        mm_remaining -= time_var * (prep.current_speed - 0.5f * speed_var); // 剩余距离 = 剩余距离 - 移动距离
                        prep.current_speed -= speed_var; // 减少当前速度
                    }
                    break;
                // 2. 加速阶段
                case Ramp_Accel:
                    speed_var = pl_block->acceleration * time_var; // v = at
                    mm_remaining -= time_var * (prep.current_speed + 0.5f * speed_var);
                    if (mm_remaining < prep.accelerate_until) {// prep.accelerate_until - 没有使用完的距离
                        mm_remaining = prep.accelerate_until;
                        time_var = 2.0f * (pl_block->millimeters - mm_remaining) / (prep.current_speed + prep.maximum_speed);
                        prep.ramp_type = mm_remaining == prep.decelerate_after ? Ramp_Decel : Ramp_Cruise;// 切换到匀速或者减速段
                        prep.current_speed = prep.maximum_speed;
                    } else {
                        prep.current_speed += speed_var;
                    }
                    break;
                // 3. 巡航阶段
                case Ramp_Cruise:
                    mm_var = mm_remaining - prep.maximum_speed * time_var;
                    if (mm_var < prep.decelerate_after) {// 巡航速度结束?
                        // t = s / v 巡航段最后一个切片消耗的时间(可能不足一个切片时间),这里记录是为了后面补偿回来
                        time_var = (mm_remaining - prep.decelerate_after) / prep.maximum_speed;
                        mm_remaining = prep.decelerate_after;
                        prep.ramp_type = Ramp_Decel;
                    } else {
                        mm_remaining = mm_var;
                    }
                    break;
                // 4. 减速阶段（默认情况）
                default: // case Ramp_Decel:
                    speed_var = pl_block->acceleration * time_var;
                    if (prep.current_speed > speed_var) {
                        mm_var = mm_remaining - time_var * (prep.current_speed - 0.5f * speed_var);
                        if (mm_var > prep.mm_complete) {// 经过deltaT时间减速后剩余的距离 > 预计减速完成后剩下的距离,成立表示减速没到位
                            mm_remaining = mm_var;
                            prep.current_speed -= speed_var;
                            break;
                        }
                    }
                    // 计算减速度段最后一个切片消耗的时间
                    time_var = 2.0f * (mm_remaining - prep.mm_complete) / (prep.current_speed + prep.exit_speed);
                    mm_remaining = prep.mm_complete;
                    prep.current_speed = prep.exit_speed;
            }
            // dt 是累加已经消耗的时间
            // dt 也不会一直累加,只要退出循环下次重入就会清零
            // dt 的值始终接近 DT_SEGMENT
            dt += time_var;
            // 这个判断是出现时间窗口不对齐的时候对齐时间窗口
            if (dt < dt_max) // 如果实际的时间片比规定的小,则循环两次修正时间窗口
                time_var = dt_max - dt;
            else {
                // minimum_mm   : 在一开始的时候初始化成 "剩余距离 - 一个脉冲距离"
                // mm_remaining : 剩余距离
                // 在DEBUG的时候发现, 正常情况下 minimum_mm = mm_remaining
                // 也就说说这个do{} while()不会一直循环,正常情况下会跳出去
                if (mm_remaining > minimum_mm) { // 这个成立证明当前运动的距离还不足一个脉冲走的距离,机器走不出这个精度
                    // 多走一个时间片,让运动距离跟机器的最小分辨率能接近
                    // 这个位置会有一点点的精度损失
                    dt_max += DT_SEGMENT;
                    time_var = dt_max - dt;
                } else
                    break; // 正常应该走这里，保证一次走一个段
            }
        // 正常情况下 prep.mm_complete = 0, 在强制减速的时候,如果在当前块内减速到0,
        // 如果还剩下一些距离就会存在 prep.mm_complete 中。

        // 在 Ramp_Decel 状态中传递给 mm_remaining，最终会保存在 pl_block->millimeters = mm_remaining;
        // mm_remaining : 上一时刻的剩余距离 - 本次delta t内移动的距离后剩下的距离, 可能会出现负值
        // 只有在切片时间没有也设定的值对齐的时候才会来到这(dt < dt_max)
        } while (mm_remaining > prep.mm_complete);


        /* -----------------------------------------------------------------------------------
           Compute spindle spindle speed for step segment
           更新主轴的百分比暂时先存起来，等待后面更新
        */
        if (sys.step_control.update_spindle_rpm || st_prep_block->dynamic_rpm) {
            // 检查是否需要更新主轴转速，或者当前预备块是否要求动态调整转速
            float rpm; // 声明变量来存储计算后的主轴转速
            // 如果主轴开启（on），继续处理转速计算
            if (pl_block->spindle.state.on) {
                // 如果启用了CSS（恒定线速度模式）
                if (pl_block->spindle.css) {
                    // 计算当前位置在总步数中的比例
                    float npos = (float)(pl_block->step_event_count - prep.steps_remaining) / (float)pl_block->step_event_count;
                    // 通过CSS公式动态计算主轴转速：初始转速 + 基于步进位置的增量转速
                    rpm = spindle_set_rpm(pl_block->spindle.hal,
                                        pl_block->spindle.rpm + pl_block->spindle.css->delta_rpm * npos,
                                        pl_block->spindle.hal->param->override_pct);
                } else {
                    // 如果没有启用CSS，则使用默认的转速计算
                    // 说明：进给（feed）和快速移动（rapid）覆写与PWM值无关，不会影响激光功率或速度
                    // 如果当前速度为零，可能需要调整转速，通常只在移动过程中发生
                    rpm = spindle_set_rpm(pl_block->spindle.hal,
                                        pl_block->condition.is_rpm_rate_adjusted && !pl_block->condition.is_laser_ppi_mode
                                            ? pl_block->spindle.rpm * prep.current_speed * prep.inv_feedrate
                                            : pl_block->spindle.rpm, 
                                        pl_block->spindle.hal->param->override_pct);
                }
            } else {
                // 如果主轴关闭，则转速为零
                pl_block->spindle.hal->param->rpm = rpm = 0.0f;
            }

            // 如果计算得到的转速与当前转速不同，则更新主轴的PWM或转速
            if (rpm != prep.current_spindle_rpm) {
                // 如果主轴硬件有PWM获取函数（用于获取当前PWM值）
                if (pl_block->spindle.hal->get_pwm != NULL) {
                    // 更新当前主轴转速
                    prep.current_spindle_rpm = rpm;
                    // 更新PWM
                    prep_segment->update_pwm = pl_block->spindle.hal->update_pwm;
                    // 这里归一化,把rpm的百分比转换成定时器初值
                    prep_segment->spindle_pwm = pl_block->spindle.hal->get_pwm(pl_block->spindle.hal, rpm);
                } else {
                    // 如果没有PWM获取函数，直接更新转速
                    prep_segment->update_rpm = pl_block->spindle.hal->update_rpm;
                    prep.current_spindle_rpm = prep_segment->spindle_rpm = rpm;
                }
                // 关闭更新主轴转速的标志
                sys.step_control.update_spindle_rpm = Off;
            }
        }


        /* -----------------------------------------------------------------------------------
           Compute segment step rate, steps to execute, and apply necessary rate corrections.
           NOTE: Steps are computed by direct scalar conversion of the millimeter distance
           remaining in the block, rather than incrementally tallying the steps executed per
           segment. This helps in removing floating point round-off issues of several additions.
           However, since floats have only 7.2 significant digits, long moves with extremely
           high step counts can exceed the precision of floats, which can lead to lost steps.
           Fortunately, this scenario is highly unlikely and unrealistic in CNC machines
           supported by Grbl (i.e. exceeding 10 meters axis travel at 200 step/mm).
        */
        /* -----------------------------------------------------------------------------------
            计算段步进率、执行的步数，并应用必要的速率修正。
            注意：步数是通过直接将剩余的毫米距离转换为步数，而不是逐步统计每个段执行的步数。这样做有助于消除多次累加中的浮动点舍入问题。
            然而，由于浮点只有7.2个有效数字，当进行极长的移动且步数非常高时，可能会超出浮点的精度，从而导致丢失步数。
            幸运的是，这种情况在Grbl支持的数控机床中是非常不可能发生的（例如，超过10米的轴移动，200步/毫米的情况下）。
        */
        /* 
            prep.steps_per_mm   : 每 mm 几个脉冲
            mm_remaining        : 剩余的距离
            step_dist_remaining : 这里计算出来的是剩余距离总共还有多少个脉冲
         */
        float step_dist_remaining = prep.steps_per_mm * mm_remaining;
        uint32_t n_steps_remaining = (uint32_t)ceilf(step_dist_remaining);/* ceilf : 浮点数向上取整 */
        /*
            prep.steps_remaining : 在第一阶段的时候就初始化成了当前段的需要多少个脉冲
                                    在后面还会更新成实际剩余多少个脉冲
            prep_segment->n_step = 总脉冲数 - 剩余脉冲数 = 当前时间片内产生多少个脉冲

            prep_segment->n_step在这里分段,当前段需要走多少步
        */
        prep_segment->n_step = (uint_fast16_t)(prep.steps_remaining - n_steps_remaining);
        if (prep_segment->n_step == 0 && sys.step_control.execute_hold) { // 调整系统状态,不是加减速的内容
            sys.step_control.end_motion = On;
            if (settings.parking.flags.enabled && !prep.recalculate.parking)
                prep.recalculate.hold_partial_block = On; // 需要保持部分块
            return;
        }
        // prep.dt_remainder 加载新的段的时候,初始化成了0
        // delta t + 上一时刻的计算误差
        dt += prep.dt_remainder;
        /*
            电机速度为V(step/min)则每步需要的时间(min/step) : T_step = 1 / V
            定时器的周期 T = 1 / f; 单位统一成分钟 T_min = 1 / (f*60)
            每步的定时器初值N = T_step / T_min = 1 / V / (1 / (f*60)) = 1 / V * (f * 60)

            prep.steps_remaining - 剩余距离总共还有多少个脉冲
            step_dist_remaining - 经过一个时间片后应该要剩下的距离
            ((float)prep.steps_remaining - step_dist_remaining) - 本次时间片应该要走的距离(step)
            inv_rate : (t / s) 这个对应 1 / V; inv_rate 就是 T_step
            cycles_per_min ： 就是 f * 60
            cycles = N = (1/V)*(f*60) = inv_rate * cycles_per_min
        */ 
        float inv_rate = dt / ((float)prep.steps_remaining - step_dist_remaining);
        // cycles_per_min : 定时器时钟频率 * 60
        uint32_t cycles = (uint32_t)ceilf(cycles_per_min * inv_rate);

        // 主轴同步相关的东西,在激光雕刻机里面用不到
        if((prep_segment->spindle_sync = pl_block->spindle.state.synchronized)) {
            prep.target_position += dt * prep.target_feed;
            prep_segment->cruising = prep.ramp_type == Ramp_Cruise;
            prep_segment->target_position = prep.target_position;
        }
        #ifdef ADAPTIVE_MULTI_AXIS_STEP_SMOOTHING
        // 自动平滑 AMASS, 当初值过小的时候不需要处理AMASS
        // 这会使得T型加减速曲线更加对称,但是加减速的首部和尾部会形成先小后大然后变小的状态
        if (cycles < amass.level_1)
            prep_segment->amass_level = 0; // 级别0：不进行平滑
        else {
            prep_segment->amass_level = cycles < amass.level_2 ? 1 : (cycles < amass.level_3 ? 2 : 3); // 根据周期数选择平滑级别
            cycles >>= prep_segment->amass_level; // 调整周期数
            prep_segment->n_step <<= prep_segment->amass_level; // 调整步数
        }
        #endif
        prep_segment->cycles_per_tick = cycles;
        prep_segment->current_rate = prep.current_speed;// 更新当前速度
        segment_buffer_head = segment_next_head;        // 移动缓存区指针
        segment_next_head = segment_next_head->next;
        pl_block->millimeters = mm_remaining;           // 这个更新很重要;在上面会把这个值重新加载成剩余距离
        prep.steps_remaining = n_steps_remaining;       // 更新剩余距离
        prep.dt_remainder = ((float)n_steps_remaining - step_dist_remaining) * inv_rate;/* (向上取整的剩余距离 - 剩余距离) * t/s(等效为1/v) */
        if (mm_remaining <= prep.mm_complete) {// 运动已经结束了
            if (mm_remaining > 0.0f) {
                sys.step_control.end_motion = On;
                if (settings.parking.flags.enabled && !prep.recalculate.parking)
                    prep.recalculate.hold_partial_block = On;
                return;
            } else {
                if (sys.step_control.execute_sys_motion) {
                    sys.step_control.end_motion = On;
                    return;
                }
                pl_block = NULL;
                plan_discard_current_block();
            }
        }
    }
}


// Called by realtime status reporting to fetch the current speed being executed. This value
// however is not exactly the current speed, but the speed computed in the last step segment
// in the segment buffer. It will always be behind by up to the number of segment blocks (-1)
// divided by the ACCELERATION TICKS PER SECOND in seconds.
float st_get_realtime_rate (void)
{
    return state_get() & (STATE_CYCLE|STATE_HOMING|STATE_HOLD|STATE_JOG|STATE_SAFETY_DOOR)
#ifdef KINEMATICS_API
            ? prep.current_speed * prep.rate_multiplier
#else
            ? prep.current_speed
#endif
            : 0.0f;
}

offset_id_t st_get_offset_id (void)
{
    plan_block_t *pl_block;

    return st.exec_block
            ? st.exec_block->offset_id
            : (sys.holding_state == Hold_Complete && (pl_block = plan_get_current_block())
                ? pl_block->offset_id
                : -1);
}
